package com.tmrnavi.myapplication;

import androidx.appcompat.app.AppCompatActivity;
import android.content.Context;
import android.content.SharedPreferences;
import android.os.Bundle;
import android.view.View;
import android.widget.Button;
import android.widget.EditText;

/**
 * Kalman Filter 参数修改与保存界面
 * 调试KF参数，保存KF参数
 */

public class KFParaSetting extends AppCompatActivity {

    private float[] KfPara = new float[27];
    private EditText[] editTextPk = new EditText[15];
    private EditText[] editTextQk = new EditText[6];
    private EditText[] editTextRk = new EditText[6];
    private Button btn_KfUpdate, btn_setDefault;
    private SharedPreferences sharedPref;
    private View.OnClickListener btnsListener;
    private SharedPreferences.Editor KFParasEditor;

    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_kfpara_setting);

        Init();     // 初始化Activity控件

        btnsListener = v -> {
            switch (v.getId())
            {
                case R.id.btn_updateKFPara:
                    getParas();
                    finish();
                    break;
                case R.id.btn_setDefaultPara:
                    setAsDefaultData();
                    break;
            }
        };
        btn_KfUpdate.setOnClickListener(btnsListener);
        btn_setDefault.setOnClickListener(btnsListener);
    }

    /************************************************
     *   初始化
     ************************************************/
    void Init()
    {
        sharedPref = this.getSharedPreferences("com.tmrnavi.myapplication.KFPARAS",
                Context.MODE_PRIVATE);              // 初始化Share Preference
        KFParasEditor = sharedPref.edit();          // 初始化Share Preference Editor
        editTextPk[0] = findViewById(R.id.et_Pk01);   editTextPk[1] = findViewById(R.id.et_Pk02);   editTextPk[2] = findViewById(R.id.et_Pk03);
        editTextPk[3] = findViewById(R.id.et_Pk04);   editTextPk[4] = findViewById(R.id.et_Pk05);   editTextPk[5] = findViewById(R.id.et_Pk06);
        editTextPk[6] = findViewById(R.id.et_Pk07);   editTextPk[7] = findViewById(R.id.et_Pk08);   editTextPk[8] = findViewById(R.id.et_Pk09);
        editTextPk[9] = findViewById(R.id.et_Pk10);   editTextPk[10] = findViewById(R.id.et_Pk11);  editTextPk[11] = findViewById(R.id.et_Pk12);
        editTextPk[12] = findViewById(R.id.et_Pk13);  editTextPk[13] = findViewById(R.id.et_Pk14);  editTextPk[14] = findViewById(R.id.et_Pk15);
        editTextQk[0] = findViewById(R.id.et_Qk1);    editTextQk[1] = findViewById(R.id.et_Qk2);    editTextQk[2] = findViewById(R.id.et_Qk3);
        editTextQk[3] = findViewById(R.id.et_Qk4);    editTextQk[4] = findViewById(R.id.et_Qk5);    editTextQk[5] = findViewById(R.id.et_Qk6);
        editTextRk[0] = findViewById(R.id.et_Rk1);    editTextRk[1] = findViewById(R.id.et_Rk2);    editTextRk[2] = findViewById(R.id.et_Rk3);
        editTextRk[3] = findViewById(R.id.et_Rk4);    editTextRk[4] = findViewById(R.id.et_Rk5);    editTextRk[5] = findViewById(R.id.et_Rk6);
        btn_KfUpdate = findViewById(R.id.btn_updateKFPara);
        btn_setDefault = findViewById(R.id.btn_setDefaultPara);
        setParas();
    }

    /************************************************
     *   显示目前保存的KF参数
     ************************************************/
    void setParas()
    {
        if (sharedPref.getFloat("Pk1",-1)>0) {
            for (int k = 0; k < 15; k++) {
                KfPara[k] = sharedPref.getFloat("Pk" + k, -1);
                editTextPk[k].setText(String.valueOf(KfPara[k]));
            }
            for (int k = 0; k < 6; k++) {
                KfPara[k + 15] = sharedPref.getFloat("Qk" + k, -1);
                editTextQk[k].setText(String.valueOf(KfPara[k + 15]));
                KfPara[k + 21] = sharedPref.getFloat("Rk" + k, -1);
                editTextRk[k].setText(String.valueOf(KfPara[k + 21]));
            }
        }
        else
        {
            setAsDefaultData();
        }

    }

    /************************************************
     *   保存输入的KF参数
     ************************************************/
    void getParas()
    {
        for (int k=0; k<15; k++)
        {
            KfPara[k] = Float.parseFloat(editTextPk[k].getText().toString());
            KFParasEditor.putFloat("Pk"+k, KfPara[k]);
        }
        for (int k=0; k<6; k++)
        {
            KfPara[k+15] = Float.parseFloat(editTextQk[k].getText().toString());
            KFParasEditor.putFloat("Qk"+k, KfPara[k+15]);
            KfPara[k+21] = Float.parseFloat(editTextRk[k].getText().toString());
            KFParasEditor.putFloat("Rk"+k, KfPara[k+21]);
        }
        KFParasEditor.commit();
    }

    /************************************************
     *   设置为App默认参数
     ************************************************/
    void setAsDefaultData()
    {
        KfPara[0] = 3f;     KfPara[1] = 3f;     KfPara[2] = 10f;            // 初始角度误差
        KfPara[3] = 0.5f;   KfPara[4] = 0.5f;   KfPara[5] = 0.5f;           // 初始速度误差
        KfPara[6] = 5f;     KfPara[7] = 5f;     KfPara[8] = 5f;             // 初始位置误差
        KfPara[9] = 500f;   KfPara[10] = 500f;  KfPara[11] = 500f;          // 初始陀螺零偏误差
        KfPara[12] = 20f;   KfPara[13] = 20f;   KfPara[14] = 20f;           // 初始加计零偏误差
        KfPara[15] = 10f;   KfPara[16] = 10f;   KfPara[17] = 10f;           // 角度随机游走
        KfPara[18] = 1f;    KfPara[19] = 1f;    KfPara[20] = 1f;            // 速度随机游走
        KfPara[21] = 1f;    KfPara[22] = 1f;    KfPara[23] = 3f;            // 速度量测噪声
        KfPara[24] = 10f;   KfPara[25] = 10f;   KfPara[26] = 30f;           // 位置量测噪声

        for (int k=0; k<15; k++)
        {
            editTextPk[k].setText(String.valueOf(KfPara[k]));
        }
        for (int k=0; k<6; k++)
        {
            editTextQk[k].setText(String.valueOf(KfPara[k + 15]));
            editTextRk[k].setText(String.valueOf(KfPara[k + 21]));
        }
    }
}